#include "cudaDistance.cuh"

__global__
void
distance(
	int *thread_num,
	DATA_TYPE *p, DATA_TYPE *point_data, INDICE_TYPE *mesh_indices,
	DATA_TYPE *result_d2, DATA_TYPE *result_nearest_point, int *result_ne)
{
	int _thread_num = *thread_num;
	DATA_TYPE triangle[SINGLE_MESH_VERTICE_COUNT][VERTICE_WIDTH];
	int index = blockIdx.x * blockDim.x + threadIdx.x;
	if (index >= _thread_num) {
		return;
	}
	DATA_TYPE *distance2 = result_d2 + index;
	DATA_TYPE *nearest_point = result_nearest_point + index * VERTICE_WIDTH;
	int *ne = result_ne + index;
	INDICE_TYPE indice;
	for (int i = 0; i < SINGLE_MESH_VERTICE_COUNT; ++i) {
		indice = *(mesh_indices + index*SINGLE_MESH_VERTICE_COUNT + i);
		for (int j = 0; j < VERTICE_WIDTH; ++j) {
			triangle[i][j] = *(point_data + indice*VERTICE_WIDTH + j);
		}
	}
	DATA_TYPE diff[VERTICE_WIDTH];
	diff[0] = triangle[0][0] - p[0];
	diff[1] = triangle[0][1] - p[1];
	diff[2] = triangle[0][2] - p[2];

	DATA_TYPE edge0[VERTICE_WIDTH];
	edge0[0] = triangle[1][0] - triangle[0][0];
	edge0[1] = triangle[1][1] - triangle[0][1];
	edge0[2] = triangle[1][2] - triangle[0][2];

	DATA_TYPE edge1[VERTICE_WIDTH];
	edge1[0] = triangle[2][0] - triangle[0][0];
	edge1[1] = triangle[2][1] - triangle[0][1];
	edge1[2] = triangle[2][2] - triangle[0][2];

	DATA_TYPE a00 = edge0[0] * edge0[0] + edge0[1] * edge0[1] + edge0[2] * edge0[2];
	DATA_TYPE a01 = edge0[0] * edge1[0] + edge0[1] * edge1[1] + edge0[2] * edge1[2];
	DATA_TYPE a11 = edge1[0] * edge1[0] + edge1[1] * edge1[1] + edge1[2] * edge1[2];
	DATA_TYPE b0 = diff[0] * edge0[0] + diff[1] * edge0[1] + diff[2] * edge0[2];
	DATA_TYPE b1 = diff[0] * edge1[0] + diff[1] * edge1[1] + diff[2] * edge1[2];
	DATA_TYPE c = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2];

	DATA_TYPE det = a00*a11 - a01*a01 > 0 ? a00*a11 - a01*a01 : a01*a01 - a00*a11;
	DATA_TYPE s = a01*b1 - a11*b0;
	DATA_TYPE t = a01*b0 - a00*b1;

	DATA_TYPE d2 = -1.0;

	if (s + t <= det)
	{
		if (s < 0)
		{
			if (t < 0)  // region 4
			{
				if (b0 < 0)
				{
					t = 0;
					if (-b0 >= a00)
					{   // VN1
						if (ne) { *ne = NEARESTENTITY_VN1; }
						s = 1;
						d2 = a00 + (2)*b0 + c;
					}
					else
					{
						// EN0
						if (ne) { *ne = NEARESTENTITY_EN0; }
						s = -b0 / a00;
						d2 = b0*s + c;
					}
				}
				else
				{
					s = 0;
					if (b1 >= 0)
					{   // VN0
						if (ne) { *ne = NEARESTENTITY_VN0; }
						t = 0;
						d2 = c;
					}
					else if (-b1 >= a11)
					{
						// VN2
						if (ne) { *ne = NEARESTENTITY_VN2; }
						t = 1;
						d2 = a11 + (2)*b1 + c;
					}
					else
					{
						// EN2
						if (ne) { *ne = NEARESTENTITY_EN2; }
						t = -b1 / a11;
						d2 = b1*t + c;
					}
				}
			}
			else  // region 3
			{
				s = 0;
				if (b1 >= 0)
				{   // VN0
					if (ne) { *ne = NEARESTENTITY_VN0; }
					t = 0;
					d2 = c;
				}
				else if (-b1 >= a11)
				{   // VN2
					if (ne) { *ne = NEARESTENTITY_VN2; }
					t = 1;
					d2 = a11 + (2)*b1 + c;
				}
				else
				{   // EN2
					if (ne) { *ne = NEARESTENTITY_EN2; }
					t = -b1 / a11;
					d2 = b1*t + c;
				}
			}
		}
		else if (t < 0)  // region 5
		{
			t = 0;
			if (b0 >= 0)
			{   // VN0
				if (ne) { *ne = NEARESTENTITY_VN0; }
				s = 0;
				d2 = c;
			}
			else if (-b0 >= a00)
			{   // VN1
				if (ne) { *ne = NEARESTENTITY_VN1; }
				s = 1;
				d2 = a00 + (2)*b0 + c;
			}
			else
			{   // EN0
				if (ne) { *ne = NEARESTENTITY_EN0; }
				s = -b0 / a00;
				d2 = b0*s + c;
			}
		}
		else  // region 0 
		{   // FN
			if (ne) { *ne = NEARESTENTITY_FN; }
			// minimum at interior point
			DATA_TYPE invDet = (1) / det;
			s *= invDet;
			t *= invDet;
			d2 = s*(a00*s + a01*t + (2)*b0) +
				t*(a01*s + a11*t + (2)*b1) + c;
		}
	}
	else
	{
		DATA_TYPE tmp0, tmp1, numer, denom;

		if (s < 0)  // region 2
		{
			tmp0 = a01 + b0;
			tmp1 = a11 + b1;
			if (tmp1 > tmp0)
			{
				numer = tmp1 - tmp0;
				denom = a00 - (2)*a01 + a11;
				if (numer >= denom)
				{   // VN1
					if (ne) { *ne = NEARESTENTITY_VN1; }
					s = 1;
					t = 0;
					d2 = a00 + (2)*b0 + c;
				}
				else
				{
					// EN1
					if (ne) { *ne = NEARESTENTITY_EN1; }
					s = numer / denom;
					t = 1 - s;
					d2 = s*(a00*s + a01*t + (2)*b0) +
						t*(a01*s + a11*t + (2)*b1) + c;
				}
			}
			else
			{
				s = 0;
				if (tmp1 <= 0)
				{   // VN2
					if (ne) { *ne = NEARESTENTITY_VN2; }
					t = 1;
					d2 = a11 + (2)*b1 + c;
				}
				else if (b1 >= 0)
				{   // VN0
					if (ne) { *ne = NEARESTENTITY_VN0; }
					t = 0;
					d2 = c;
				}
				else
				{
					// EN2
					if (ne) { *ne = NEARESTENTITY_EN2; }
					t = -b1 / a11;
					d2 = b1*t + c;
				}
			}
		}
		else if (t < 0)  // region 6
		{
			tmp0 = a01 + b1;
			tmp1 = a00 + b0;
			if (tmp1 > tmp0)
			{
				numer = tmp1 - tmp0;
				denom = a00 - (2)*a01 + a11;
				if (numer >= denom)
				{   // VN2
					if (ne) { *ne = NEARESTENTITY_VN2; }
					t = 1;
					s = 0;
					d2 = a11 + (2)*b1 + c;
				}
				else
				{
					// EN1
					if (ne) { *ne = NEARESTENTITY_EN1; }
					t = numer / denom;
					s = 1 - t;
					d2 = s*(a00*s + a01*t + (2)*b0) +
						t*(a01*s + a11*t + (2)*b1) + c;
				}
			}
			else
			{
				t = 0;
				if (tmp1 <= 0)
				{   // VN1
					if (ne) { *ne = NEARESTENTITY_VN1; }
					s = 1;
					d2 = a00 + (2)*b0 + c;
				}
				else if (b0 >= 0)
				{   // VN0
					if (ne) { *ne = NEARESTENTITY_VN0; }
					s = 0;
					d2 = c;
				}
				else
				{
					// EN0
					if (ne) { *ne = NEARESTENTITY_EN0; }
					s = -b0 / a00;
					d2 = b0*s + c;
				}
			}
		}
		else  // region 1
		{
			numer = a11 + b1 - a01 - b0;
			if (numer <= 0)
			{   // VN2
				if (ne) { *ne = NEARESTENTITY_VN2; }
				s = 0;
				t = 1;
				d2 = a11 + (2)*b1 + c;
			}
			else
			{
				denom = a00 - (2)*a01 + a11;
				if (numer >= denom)
				{   // VN1
					if (ne) { *ne = NEARESTENTITY_VN1; }
					s = 1;
					t = 0;
					d2 = a00 + (2)*b0 + c;
				}
				else
				{   // EN1
					if (ne) { *ne = NEARESTENTITY_EN1; }
					s = numer / denom;
					t = 1 - s;
					d2 = s*(a00*s + a01*t + (2)*b0) +
						t*(a01*s + a11*t + (2)*b1) + c;
				}
			}
		}
	}

	// Account for numerical round-off error.
	if (d2 < 0)
	{
		d2 = 0;
	}

	if (nearest_point) {
		nearest_point[0] = triangle[0][0] + s*edge0[0] + t*edge1[0];
		nearest_point[1] = triangle[0][1] + s*edge0[1] + t*edge1[1];
		nearest_point[2] = triangle[0][2] + s*edge0[2] + t*edge1[2];
	}

	*distance2 = d2;
}

__global__
void
min_reduce1(
	int *thread_num,
	DATA_TYPE *arr, int *ids
)
{
	__shared__ DATA_TYPE arr_in_block[BLOCK_WIDTH];
	__shared__ int id_in_block[BLOCK_WIDTH];
	__shared__ int THREAD_CNT;
	int _thread_num = *thread_num;
	int index = blockIdx.x * blockDim.x + threadIdx.x;
	if (index < _thread_num) {
		int sub_index = threadIdx.x;

		id_in_block[sub_index] = index;
		arr_in_block[sub_index] = arr[index];
		if (sub_index == 0) {
			THREAD_CNT = _thread_num >= blockDim.x*(blockIdx.x + 1) ? blockDim.x : _thread_num - blockDim.x * blockIdx.x;
		}
		__syncthreads();

		int i = THREAD_CNT;
		while (i>1) {
			if (sub_index == 0 && i % 2 != 0) {
				if (arr_in_block[0] > arr_in_block[i - 1]) {
					arr_in_block[0] = arr_in_block[i - 1];
					id_in_block[0] = id_in_block[i - 1];
				}
			}
			i >>= 1;
			if (sub_index < i) {
				if (arr_in_block[sub_index] > arr_in_block[sub_index + i]) {
					arr_in_block[sub_index] = arr_in_block[sub_index + i];
					id_in_block[sub_index] = id_in_block[sub_index + i];
				}
			}
			__syncthreads();
		}
		if (sub_index == 0) {
			ids[blockIdx.x] = id_in_block[sub_index];
		}
	}
}

__global__
void
min_reduce2(
	int *block_num, int *ids,
	DATA_TYPE *arr_d2, DATA_TYPE *arr_nearest_point, int *arr_ne)
{
	int _block_num = *block_num;
	DATA_TYPE _d2 = arr_d2[ids[0]];
	DATA_TYPE tmp_d2;
	int id_index = 0;
	for (int i = 1; i < _block_num; ++i) {
		tmp_d2 = arr_d2[ids[i]];
		if (_d2 > arr_d2[ids[i]]) {
			_d2 = tmp_d2;
			id_index = i;
		}
	}
	int _id = ids[id_index];
	*ids = _id;
	*arr_d2 = _d2;
	DATA_TYPE *_arr_nearest_point = arr_nearest_point + VERTICE_WIDTH*_id;
	for (int i = 0; i < VERTICE_WIDTH; ++i) {
		arr_nearest_point[i] = _arr_nearest_point[i];
	}
	int *_ne = arr_ne + _id;
	for (int i = 0; i < NE_WIDTH; ++i) {
		arr_ne[i] = _ne[i];
	}
}

extern "C" int run(
	DATA_TYPE p_x, DATA_TYPE p_y, DATA_TYPE p_z,
	DATA_TYPE *point_data, int point_count, INDICE_TYPE *mesh_indices, int mesh_count,
	int *id, DATA_TYPE *d2, DATA_TYPE *nearest_point, int *ne)
{
	// declare and allocate host memory
	int thread_num = mesh_count;
	DATA_TYPE h_p[VERTICE_WIDTH];
	h_p[0] = p_x;
	h_p[1] = p_y;
	h_p[2] = p_z;

	// step1 calculate distance per mesh
	// declare, allocate, and copy CPU memory to GPU memory
	DATA_TYPE *d_p;
	int VERTICE_DATA_BYTES = VERTICE_WIDTH * sizeof(DATA_TYPE);
	cudaMalloc((void **)&d_p, VERTICE_DATA_BYTES);
	cudaMemcpy(d_p, h_p, VERTICE_DATA_BYTES, cudaMemcpyHostToDevice);
	DATA_TYPE *d_point_data;
	int MESH_DATA_BYTES = point_count * VERTICE_WIDTH * sizeof(DATA_TYPE);
	cudaMalloc((void**)&d_point_data, MESH_DATA_BYTES);
	cudaMemcpy(d_point_data, point_data, MESH_DATA_BYTES, cudaMemcpyHostToDevice);
	INDICE_TYPE *d_mesh_indices;
	int MESH_INDICES_BYTES = mesh_count * SINGLE_MESH_VERTICE_COUNT * sizeof(INDICE_TYPE);
	cudaMalloc((void**)&d_mesh_indices, MESH_INDICES_BYTES);
	cudaMemcpy(d_mesh_indices, mesh_indices, MESH_INDICES_BYTES, cudaMemcpyHostToDevice);

	DATA_TYPE *d_result_d2;
	int RESULT_D2_BYTES = thread_num * sizeof(DATA_TYPE);
	cudaMalloc((void**)&d_result_d2, RESULT_D2_BYTES);
	cudaMemset((void*)d_result_d2, 0, RESULT_D2_BYTES);
	DATA_TYPE *d_result_nearest_point;
	int RESULT_NEAREST_POINT_BYTES = thread_num * VERTICE_WIDTH * sizeof(DATA_TYPE);
	cudaMalloc((void**)&d_result_nearest_point, RESULT_NEAREST_POINT_BYTES);
	cudaMemset((void*)d_result_nearest_point, 0, RESULT_NEAREST_POINT_BYTES);
	int *d_result_ne;
	int RESULT_NE_BYTES = thread_num * sizeof(int);
	cudaMalloc((void**)&d_result_ne, RESULT_NE_BYTES);
	cudaMemset((void*)d_result_ne, '\0', RESULT_NE_BYTES);
	int *d_thread_num;
	cudaMalloc((void**)&d_thread_num, sizeof(int));
	cudaMemcpy(d_thread_num, &thread_num, sizeof(int), cudaMemcpyHostToDevice);
	int *d_block_num;
	int BLOCK_COUNT = thread_num % BLOCK_WIDTH == 0 ? thread_num / BLOCK_WIDTH : thread_num / BLOCK_WIDTH + 1;
	cudaMalloc((void**)&d_block_num, sizeof(int));
	cudaMemcpy(d_block_num, &BLOCK_COUNT, sizeof(int), cudaMemcpyHostToDevice);

	// launch the kernel
#ifdef PRINT_INFO
	printf("block count is %d\n", BLOCK_COUNT);
	printf("kernel run - #distance# %d threads, %d blocks, %d threads each\n",
		thread_num, BLOCK_COUNT, BLOCK_WIDTH);
#endif // PRINT_INFO
	GpuTimer timer1;
	timer1.Start();
	distance << < BLOCK_COUNT, BLOCK_WIDTH >> > (d_thread_num, d_p, d_point_data, d_mesh_indices, d_result_d2, d_result_nearest_point, d_result_ne);
	timer1.Stop();

	// step2 reduce by minimum
	int *h_result_id;
	h_result_id = (int*)malloc(sizeof(int)*BLOCK_COUNT);
	int *d_result_id;
	int RESULT_ID_BYTES = BLOCK_COUNT * sizeof(int);
	cudaMalloc((void**)&d_result_id, RESULT_ID_BYTES);
	cudaMemset((void*)d_result_id, 0, RESULT_ID_BYTES);

#ifdef PRINT_INFO
	printf("kernel run - #min_reduce1# %d threads, %d blocks, %d threads each\n",
		thread_num, BLOCK_COUNT, BLOCK_WIDTH);
#endif // PRINT_INFO
	GpuTimer timer2;
	timer2.Start();
	min_reduce1 << < BLOCK_COUNT, BLOCK_WIDTH >> > (d_thread_num, d_result_d2, d_result_id);
	timer2.Stop();

#ifdef PRINT_INFO
	printf("kernel run - #min_reduce2# %d threads, %d blocks, %d threads each\n",
		1, 1, 1);
#endif // PRINT_INFO
	GpuTimer timer3;
	timer3.Start();
	min_reduce2 << < 1, 1 >> > (d_block_num, d_result_id, d_result_d2, d_result_nearest_point, d_result_ne);
	timer3.Stop();

	cudaMemcpy(id, d_result_id, sizeof(int), cudaMemcpyDeviceToHost);
	cudaMemcpy(d2, d_result_d2, sizeof(DATA_TYPE), cudaMemcpyDeviceToHost);
	cudaMemcpy(nearest_point, d_result_nearest_point, VERTICE_DATA_BYTES, cudaMemcpyDeviceToHost);
	cudaMemcpy(ne, d_result_ne, sizeof(int), cudaMemcpyDeviceToHost);

	// print data
#ifdef PRINT_INFO
	printf("\npoint is ( %g, %g, %g )\n", h_p[0], h_p[1], h_p[2]);
	printf("index of the nearest mesh is %d :\n", *id);
	for (int j = 0; j < SINGLE_MESH_VERTICE_COUNT; ++j) {
		printf("\t( ");
		for (int k = 0; k < VERTICE_WIDTH; ++k) {
			if (k == VERTICE_WIDTH - 1) {
				printf("%g )", point_data[mesh_indices[*id*SINGLE_MESH_VERTICE_COUNT + j] * VERTICE_WIDTH + k]);
			}
			else {
				printf("%g, ", point_data[mesh_indices[*id*SINGLE_MESH_VERTICE_COUNT + j] * VERTICE_WIDTH + k]);
			}
		}
		putchar('\n');
	}
	printf("d2 is %f\n", *d2);
	printf("ne is -%d-\n", *ne);
	printf("nearest point is:( ");
	for (int l = 0; l < VERTICE_WIDTH; ++l) {
		if (l == VERTICE_WIDTH - 1) {
			printf("%g )", nearest_point[l]);
		}
		else {
			printf("%g, ", nearest_point[l]);
		}
	}
	printf("\n");
	printf("\nTime elapsed = %g ms + %g ms + %g ms\n\n", timer1.Elapsed(), timer2.Elapsed(), timer3.Elapsed());
#endif // PRINT_INFO

	free(h_result_id);
	// free GPU memory allocation and exit
	cudaFree(d_result_id);
	cudaFree(d_block_num);
	cudaFree(d_thread_num);
	cudaFree(d_result_ne);
	cudaFree(d_result_nearest_point);
	cudaFree(d_result_d2);
	cudaFree(d_mesh_indices);
	cudaFree(d_point_data);
	cudaFree(d_p);
	return 0;
}